#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

MPU6050 mpu(0x68);

/* 中断标记 */
volatile bool interruptState = true;

/* 记录 MPU6050 DMP 中断状态 */
uint8_t mpuIntStatus;

/* 用于读取 MPU6050 FIFO 数据 */
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];

/* 记录 MPU6050 DMP 初始化是否成功, 决定 loop 函数是否执行 */
bool dmpReady = false;

Quaternion q;
VectorFloat gravity;
float ypr[3];
float angle;                                // 平衡小车倾角

void setup()
{
  Wire.begin();
  Serial.begin(115200);

  /* 初始化 MPU6050 */
  mpu.initialize();
  delay(5);

  /* 初始化 MPU6050, 启用DMP */
  if (mpu.dmpInitialize())
  {
    Serial.println("err: dmp int failed!");
    return;
  }

  mpu.setDMPEnabled(true);
  dmpReady = true;
  packetSize = mpu.dmpGetFIFOPacketSize();

  /* 设置 PIN2 上升沿中断为 DMP 数据就绪中断 */
  attachInterrupt(0, mpu6050_handler, RISING);
}

void loop()
{
  /* 判断初始化(setup函数)阶段MPU6050 DMP初始化配置是否成功 */
  /* 不成功的, 下面的代码就直接不运行了 */
  if (!dmpReady)
  {
    return;
  }

  /* MPU6050 中断来了，则 mpuInterrupt 置位为 true */
  if (!interruptState && fifoCount < packetSize)
  {
    return;
  }

  interruptState = false;
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();
  if ((mpuIntStatus & 0x10) || fifoCount == 1024)
  {
    /* 异常状态, 清空 FIFO */
    mpu.resetFIFO();
  }
  else if (mpuIntStatus & 0x02)
  {
    /* 正常状态, 获取 FIFO 数据 */
    /* FIFO 数据没有拿全, 则继续拿一点数据 */
    while (fifoCount < packetSize)
    {
      fifoCount = mpu.getFIFOCount();
    }
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    fifoCount -= packetSize;

    /* 获取四元数, 用于获取重力加速度和欧拉角  */
    mpu.dmpGetQuaternion(&q, fifoBuffer);

    /* 获取重力加速度, 用于获取欧拉角 */
    mpu.dmpGetGravity(&gravity, &q);

    /* 获取欧拉角,.需要使用上面两个值, ypr[3] 表示三个欧拉角度 */
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    /* 计算平衡小车倾角 */
    angle = -ypr[2] * 180 / M_PI;
  }

  Serial.print("angle = ");
  Serial.println(angle);

}

void mpu6050_handler()
{
  interruptState = true;
}
